/**
 * COPYRIGHT NOTICE Copyright (C) 2017, Buffalo Robot Tech.co, Ltd All rights
 * reserved.
 *
 * @file main.c
 * @brief      This is the test file of spline trajectory
 *
 * @author     chauby 
 * @email    chaubyZou@163.com
 * @date       Dec.22, 2017
 * @version    Rehab v1.0.0.0
 */
#include <stdio.h>
#include <math.h>
#include "cubic_spline.h"

#define POINT_NUM 10
/**
 * @brief test of serial point's trajectory generation
 * @details [long description]
 */
void cubicSplineSerialPointsTest(void)
{
    printf("serial points spline trajectory test\n");

    float t[POINT_NUM] = {0.3, 0.6, 1.2, 2.0, 2.8, 3.3, 4.5, 5.1, 6.0, 7.5};
    float p[POINT_NUM] = {sin(t[0]), sin(t[1]), sin(t[2]), sin(t[3]), sin(t[4]), sin(t[5]), sin(t[6]), sin(t[7]), sin(t[8]), sin(t[9])};
    float v[POINT_NUM] = {cos(t[0]), cos(t[1]), cos(t[2]), cos(t[3]), cos(t[4]), cos(t[5]), cos(t[6]), cos(t[7]), cos(t[8]), cos(t[9])};

    PVTPoint given_point[POINT_NUM] = {0};
    for(unsigned int index=0; index < POINT_NUM; index++)
    {
        given_point[index].position = p[index];
        given_point[index].velocity = v[index];
        given_point[index].time = t[index];
    }

    CubicSpline serial_spline = {0};
    initializeCubicSpline(&serial_spline, "SerialSpline");

    for(unsigned int index=0; index<POINT_NUM; index++)
    {
        addPointToCubicSplineQueue(&serial_spline, given_point[index]);
    }

    // showCubicSplineQueue(&serial_spline);

    FILE *fd1 = fopen("../DataAnalysis/SplineFiles/given_point.txt", "w+");
    for(unsigned int index=0; index < POINT_NUM; index++)
    {
        fprintf(fd1, "%f %f %f\n", t[index], p[index], v[index]); ///< write the given point to file
    }
    fclose(fd1);

    FILE *fd2 = fopen("../DataAnalysis/SplineFiles/spline_trajectory.txt", "w+");
    float deltaT = 0.001;
    PVTPoint pvt_point = {0};

    for(float t=-1; t<10; t+=deltaT)
    {
        pvt_point = getCubicSplineValue(&serial_spline, t);
        fprintf(fd2, "%f %f %f\n", pvt_point.time, pvt_point.position, pvt_point.velocity); ///< write the trajectory to file
    }

    fclose(fd2);
}

/**
 * @brief main function
 * @details [long description]
 */
void main(void)
{
    cubicSplineSerialPointsTest();
}
